#include "preintegration/imu_preintegration.h"

namespace preintegration {

ImuPreIntegration::ImuPreIntegration(ImuOptions options) {
    bg_ = options.init_bg_;
    ba_ = options.init_ba_;

    const double ng2 = options.noise_gyro_ * options.noise_gyro_;
    const double na2 = options.noise_acce_ * options.noise_acce_;

    noise_gyro_acce_ << ng2, na2, na2;

    // cov_ = 1e-2 * Eigen::Matrix<double, 5, 5>::Ones();
    cov_ = 1e-2 * Eigen::Matrix<double, 5, 5>::Identity();

    // LOG(INFO) << "cov_ \n" << cov_ << std::endl;
}

void ImuPreIntegration::Integrate(const Eigen::Vector3d& last_imu, const Eigen::Vector3d& curr_imu, const double& dt) {
    Eigen::Vector3d last_imu_unbias(last_imu[0], last_imu[1], last_imu[2]);
    Eigen::Vector3d curr_imu_unbias(curr_imu[0], curr_imu[1], curr_imu[2]);
    last_imu_unbias[0] -= ba_[0];
    curr_imu_unbias[0] -= ba_[0];
    last_imu_unbias[1] -= ba_[1];
    curr_imu_unbias[1] -= ba_[1];
    last_imu_unbias[2] -= bg_;
    curr_imu_unbias[2] -= bg_;
    
    Eigen::Vector3d imu_unbias_hat = 0.5 * (last_imu_unbias + curr_imu_unbias);
    Eigen::Vector2d acc_unbias = imu_unbias_hat.head<2>();
    double gyr = imu_unbias_hat[2];

    // （七）
    dp_ = dp_ + dv_ * dt + 0.5f * dR_.matrix() * acc_unbias * dt * dt; // (2-1)
    dv_ = dv_ + dR_.matrix() * acc_unbias * dt; // (2-2)

    Eigen::Matrix<double, 5, 5> A;
    A.setZero();

    Eigen::Matrix2d one_hat = SO2::so2(1);
    double dt2 = dt * dt;
    A(0,0) = 1; // (6)
    A.block<2,1>(1,0) = -dR_.matrix() * (one_hat * acc_unbias * dR_.log() * dt); // (7)
    A.block<2,2>(1,1) = Eigen::Matrix2d::Identity(); // (7)
    A.block<2,1>(3,0) = -0.5 * dR_.matrix() * one_hat * acc_unbias * dt2; // (8)
    A.block<2,2>(3,1) = Eigen::Matrix2d::Identity() * dt; // (8)
    A.block<2,2>(3,3) = Eigen::Matrix2d::Identity(); // (8)

    Eigen::Matrix<double, 5, 3> B;
    B.setZero();
    B(0,0) = dt; // (6)
    B.block<2,2>(1,1) = dR_.matrix() * dt; // (7)
    B.block<2,2>(3,1) = 0.5 * dR_.matrix() * dt2; // (8)

    // （八）
    dP_dba_ = dP_dba_ + dV_dba_ * dt - 0.5f * dR_.matrix() * dt2; // (28)
    dP_dbg_ = dP_dbg_ + dV_dbg_ * dt - 0.5f * dR_.matrix() * dt2 * one_hat * dR_dbg_ * acc_unbias; // (29)
    dV_dba_ = dV_dba_ - dR_.matrix() * dt; // (26)
    dV_dbg_ = dV_dbg_ - dR_.matrix() * dt * one_hat * dR_dbg_ * acc_unbias; // (27)
    dR_dbg_ = dR_dbg_ + dt; // (25)

    // （七）
    double omega = gyr * dt;

    // cout << " omega ==== : " << omega << std::endl;

    SO2 deltaR = SO2(omega);

    // cout << " deltaR ==== : " << deltaR.log() << std::endl;

    dR_ = dR_ * deltaR;             // (2-3)

    cov_ = A * cov_ * A.transpose() + B * noise_gyro_acce_ * B.transpose();

    // 此处imu由于频率较低，用固定协方差矩阵，否则算法很难收敛，若imu本身频率高，可去掉这一行
    // cov_ = 1e5 * Eigen::Matrix<double, 5, 5>::Identity();
    // cout << " cov_ ==== : " << cov_ << std::endl;
    // cout << " dt ==== : " << dt << std::endl;

    dt_ += dt;
}

State ImuPreIntegration::Predict(const State& last_state) {

    // cout << " last_state.v ==== : " << last_state.v << std::endl;

    State state;
    state.timestamp = last_state.timestamp + dt_;
    SO2 R = last_state.R;
    state.R = R * dR_;
    state.v = R.matrix() * dv_ + last_state.v;
    state.p = R.matrix() * dp_ + last_state.p + last_state.v * dt_;

    // cout << " state.v ==== : " << state.v << std::endl;
    // cout << " R.matrix() ==== : " << R.matrix() << std::endl;
    // cout << " dt_ ==== : " << dt_ << std::endl;
    // cout << " dp_ ==== : " << dp_ << std::endl;
    // cout << " dR_ ==== : " << dR_.log() << std::endl;

    state.bg = bg_;
    state.ba = ba_;

    return state;
}

//（八） (13)
SO2 ImuPreIntegration::GetDeltaRotation(const double &bg) {
    return dR_ * SO2::exp(dR_dbg_ * (bg - bg_));
}

//（八） (13)
Eigen::Vector2d ImuPreIntegration::GetDeltaVelocity(const double &bg, const Eigen::Vector2d &ba) {
    return dv_ + dV_dbg_ * (bg - bg_) + dV_dba_ * (ba - ba_);
}
//（八） (13)
Eigen::Vector2d ImuPreIntegration::GetDeltaPosition(const double &bg, const Eigen::Vector2d &ba) {
    return dp_ + dP_dbg_ * (bg - bg_) + dP_dba_ * (ba - ba_);
}

void ImuPreIntegration::Clear(const ImuOptions& options) {
    dt_ = 0; 
    cov_ = Eigen::Matrix<double, 5, 5>::Zero();
    noise_gyro_acce_ = Eigen::Matrix3d::Zero();

    dR_ = SO2();
    dv_ = Eigen::Vector2d::Zero();
    dp_ = Eigen::Vector2d::Zero();  

    dR_dbg_ = 0;
    dV_dbg_ = Eigen::Vector2d::Zero();
    dV_dba_ = Eigen::Matrix2d::Zero();
    dP_dbg_ = Eigen::Vector2d::Zero();
    dP_dba_ = Eigen::Matrix2d::Zero(); 

    bg_ = options.init_bg_;
    ba_ = options.init_ba_;

    const double ng2 = options.noise_gyro_ * options.noise_gyro_;
    const double na2 = options.noise_acce_ * options.noise_acce_;

    noise_gyro_acce_ << ng2, na2, na2;

}

}